#include <mpu6050.h>

int mpu6050_init(){
    int ret=0,id=0;
    mpuiic_init(MPU_ADDR); 
    if((id=mpuiic_read(MPU_ADDR,MPU_DEVICE_ID_REG))!=MPU_ADDR){
        printf("ID ERROR:MPU_DEVICE_ID = %d\r\n",id);
        return -1;
    }
    ret+=mpuiic_write(MPU_ADDR,MPU_PWR_MGMT1_REG,0x80);//复位
    msleep(100);
    ret+=mpuiic_write(MPU_ADDR,MPU_PWR_MGMT1_REG,0x00);//唤醒并设置时钟源为x轴PLL时钟
    ret+=mpuiic_write(MPU_ADDR,MPU_GYRO_CFG_REG,0x18);//设置陀螺仪量程 3<<3 0:±250°/s 1:±500°/s 2:±1000°/s 3:±2000°/s
    ret+=mpuiic_write(MPU_ADDR,MPU_ACCEL_CFG_REG,0x08);//设置加速度计量程 1<<3 0:±2g 1:±4g 2:±8g 3:±9g
    ret+=mpuiic_write(MPU_ADDR,MPU_CFG_REG,0x04);//设置低通滤波器
    ret+=mpuiic_write(MPU_ADDR,MPU_SAMPLE_RATE_REG,0x00);//设置采样频率 100hz
    ret+=mpuiic_write(MPU_ADDR,MPU_INT_EN_REG,0x00);//关闭所有中断
	ret+=mpuiic_write(MPU_ADDR,MPU_USER_CTRL_REG,0X00);	//I2C主模式关闭
	ret+=mpuiic_write(MPU_ADDR,MPU_FIFO_EN_REG,0X00);	//关闭FIFO
	ret+=mpuiic_write(MPU_ADDR,MPU_INTBP_CFG_REG,0X80);	//INT引脚低电平有效
    ret+=mpuiic_write(MPU_ADDR,MPU_PWR_MGMT1_REG,0X01);	//设置CLKSEL,PLL X轴为参考
    ret+=mpuiic_write(MPU_ADDR,MPU_PWR_MGMT2_REG,0X00);	//加速度与陀螺仪都工作
    if(ret!=0){
        printf("MPU INIT:ERROR\r\n");
        return -1;
    }
    printf("MPU INIT:PASS\r\n");
    return 0;
}
int MPU_Get_Gyroscope(int16_t *g)
{
    int ret=0;
    uint8_t buf[6];  
	ret=mpuiic_read_len(MPU_ADDR,MPU_GYRO_XOUTH_REG,6,buf);
	if(ret==0)
	{
	    g[0]=((uint16_t)buf[0]<<8)|buf[1];  
		g[1]=((uint16_t)buf[2]<<8)|buf[3];  
		g[2]=((uint16_t)buf[4]<<8)|buf[5];
    }
    return ret;
}
int MPU_Get_Accelerometer(int16_t *a)
{
    int ret=0;  
    uint8_t buf[6];
	ret=mpuiic_read_len(MPU_ADDR,MPU_ACCEL_XOUTH_REG,6,buf);
	if(ret==0)
	{
		a[0]=((uint16_t)buf[0]<<8)|buf[1];  
		a[1]=((uint16_t)buf[2]<<8)|buf[3];  
		a[2]=((uint16_t)buf[4]<<8)|buf[5];
	}
    return ret;
}